#include "Geometry.h"

using namespace maths;

CLine::CLine(CVector4f &a_Dir, CVector4f &a_Point)
{
	m_Dir = a_Dir;
	m_Point = a_Point;
}

CLine::CLine()
{
	m_Dir[3] = 1.0;
}

CVector4f& CLine::Dir()
{
	return m_Dir;
}

CVector4f& CLine::Point()
{
	return m_Point;
}

CPlane::CPlane()
{
	m_NandD[3] = 1.0f;
}

CPlane::CPlane(CVector4f &a_NandD)
{
	m_NandD = a_NandD;
}

CPlane::CPlane(CVector4f &a_Norm, mFloat a_D)
{
	m_NandD = a_Norm;
	m_NandD[4] = a_D;
}

mVoid CPlane::SetD(mFloat a_D)
{
	m_NandD[4] = a_D;
}

mFloat CPlane::GetD()
{
	return m_NandD[4];
}

mVoid CPlane::SetN(CVector4f &a_Norm)
{
	m_NandD[0] = a_Norm[0];
	m_NandD[1] = a_Norm[1];
	m_NandD[2] = a_Norm[2];
}

mVoid CPlane::SetN(mFloat x, mFloat y, mFloat z)
{
	m_NandD[0] = x;
	m_NandD[1] = y;
	m_NandD[2] = z;
}

CVector4f CPlane::GetN()
{
	return CVector4f(m_NandD[0], m_NandD[1], m_NandD[2], 0.0f);
}

CAABox::CAABox()
{

}

mBool CAABox::Collide(CAABox &a_Collider)
{
	CVector4f &Min = a_Collider.Min(), &Max = a_Collider.Max();
	mInt32 Result;
	__asm
	{
		mov eax, dword ptr m_Min
		movaps xmm0, xmmword ptr [eax]
		mov ecx, dword ptr m_Max
		movaps xmm1, xmmword ptr [ecx]

		mov edx, dword ptr Min
		movaps xmm2, xmmword ptr [edx]
		mov edx, dword ptr Max
		movaps xmm3, xmmword ptr [edx]

		maxps xmm0, xmm2
		movaps xmmword ptr [eax], xmm0
		minps xmm1, xmm3
		movaps xmmword ptr [ecx], xmm1

		cmpleps xmm0, xmm1
		pshufd  xmm0, xmm1, 10110001b
		andps   xmm0, xmm1
		pshufd  xmm0, xmm1, 01001110b
		andps   xmm0, xmm1

		mov eax, dword ptr Result
		movss xmmword ptr [eax], xmm0
	}

	if (Result != 0)
		return true;
	else
		return false;
}

CAABox::CAABox(CVector4f &a_Min, CVector4f &a_Max)
{
	m_Max = a_Max;
	m_Min = a_Min;
}

CVector4f& CAABox::Min()
{
	return m_Min;
}

CVector4f& CAABox::Max()
{
	return m_Max;
}

mVoid CAABox::GetPoints(CPointArray &a_Res)
{
	a_Res.Clear();
	a_Res.Push(CVector4f( m_Min[0], m_Min[1], m_Min[2], 1.0f ));//     7+------+6
	a_Res.Push(CVector4f( m_Max[0], m_Min[1], m_Min[2], 1.0f ));//     /|     /|
	a_Res.Push(CVector4f( m_Min[0], m_Max[1], m_Min[2], 1.0f ));//    / |    / |
	a_Res.Push(CVector4f( m_Max[0], m_Max[1], m_Min[2], 1.0f ));//   / 4+---/--+5
	a_Res.Push(CVector4f( m_Min[0], m_Min[1], m_Max[2], 1.0f ));// 3+------+2 /    y  z
	a_Res.Push(CVector4f( m_Max[0], m_Min[1], m_Max[2], 1.0f ));//  | /    | /     | /
	a_Res.Push(CVector4f( m_Min[0], m_Max[1], m_Max[2], 1.0f ));//  |/     |/      |/
	a_Res.Push(CVector4f( m_Max[0], m_Max[1], m_Max[2], 1.0f ));// 0+------+1      *---x
}

mBool CAABox::LastLineISect(CVector4f &a_Res, CLine &a_Line)
{
	const float l_fEps = 0.01f;
	float l_fCoef;
	CVector4f l_vISectPoint;

	if (!com::equal(a_Line.Dir()[0], 0.0f, l_fEps))
	{
		if (a_Line.Dir()[0] > 0.0f)
		{
			l_fCoef = (m_Max[0] - a_Line.Point()[0]) / a_Line.Dir()[0];
		}
		else
		{
			l_fCoef = (m_Min[0] - a_Line.Point()[0]) / a_Line.Dir()[0];
		}
		// Point a_Line.Point + l_fCoef * a_Line.Dir lies on the BBox plane

		l_vISectPoint = a_Line.Point() + a_Line.Dir() * l_fCoef;
		if ((l_vISectPoint[1] >= m_Min[1]) && (l_vISectPoint[1] <= m_Max[1]) && (l_vISectPoint[2] >= m_Min[2]) && (l_vISectPoint[2] <= m_Max[2]))
		{
			a_Res = l_vISectPoint;
			return true;
		}         
	}

	if (!com::equal(a_Line.Dir()[1], 0.0f, l_fEps))
	{
		if (a_Line.Dir()[1] > 0.0f)
		{
			l_fCoef = (m_Max[1] - a_Line.Point()[1]) / a_Line.Dir()[1];
		}
		else
		{
			l_fCoef = (m_Min[1] - a_Line.Point()[1]) / a_Line.Dir()[1];
		}
		// Point a_Line.Point + l_fCoef * a_Line.Dir lies on the BBox plane

		l_vISectPoint = a_Line.Point() + a_Line.Dir() * l_fCoef;
		if ((l_vISectPoint[0] >= m_Min[0]) && (l_vISectPoint[0] <= m_Max[0]) && (l_vISectPoint[2] >= m_Min[2]) && (l_vISectPoint[2] <= m_Max[2]))
		{
			a_Res = l_vISectPoint;
			return true;
		}         
	}

	if (!com::equal(a_Line.Dir()[2], 0.0f, l_fEps))
	{
		if (a_Line.Dir()[2] > 0.0f)
		{
			l_fCoef = (m_Max[2] - a_Line.Point()[2]) / a_Line.Dir()[2];
		}
		else
		{
			l_fCoef = (m_Min[2] - a_Line.Point()[2]) / a_Line.Dir()[2];
		}
		// Point a_Line.Point + l_fCoef * a_Line.Dir lies on the BBox plane

		l_vISectPoint = a_Line.Point() + a_Line.Dir() * l_fCoef;
		if ((l_vISectPoint[0] >= m_Min[0]) && (l_vISectPoint[0] <= m_Max[0]) && (l_vISectPoint[1] >= m_Min[1]) && (l_vISectPoint[1] <= m_Max[1]))
		{
			a_Res = l_vISectPoint;
			return true;
		}         
	}
	return false;
}

mVoid CPointArray::Push( CPointArray &a_Array )
{
	//m_Points.insert(m_Points.end(), a_Array.m_Points.begin(), a_Array.m_Points.end());
    for (mUInt i = 0; i < a_Array.Size(); i++)
    {
        m_Points.AppendObj(&a_Array[i]);
    }
}

mVoid CPointArray::Push( CVector4f &a_Point )
{
	m_Points.AppendObj(&a_Point);
}

mVoid CPointArray::Clear( )
{
	//m_Points.clear();
    m_Points.SetSize(0);
}

mUInt CPointArray::Size( )
{
    return m_Points.GetSize();
}

/*std::vector < CVector4f > :: iterator CPointArray::Begin()
{
	return m_Points.begin();
}

std::vector < CVector4f > :: iterator CPointArray::End()
{
	return m_Points.end();
}*/

CVector4f & CPointArray :: operator [](mUInt32 a_Ind)
{
	return m_Points[a_Ind];
}

mVoid CPointArray::Transform(CMatrix4x4f &a_Mtx)
{
	//std::vector < CVector4f > :: iterator PointIt;
    //CVector4f *pPoint;
	//for (PointIt = m_Points.begin(); PointIt !=  m_Points.end(); ++PointIt)
    for (mUInt i = 0; i < m_Points.GetSize(); i++)
	{
        m_Points[i] *= a_Mtx;
	}
}

mVoid CPointArray::CalcAABBox(CAABox &a_Res)
{
	CVector4f &Min = a_Res.Min(), &Max = a_Res.Max();
	Max = Min = m_Points[0];//*m_Points.begin();
	
	for (mUInt i = 0; i < m_Points.GetSize(); i++)
	{
		__asm
		{
			mov eax, dword ptr Min
			movaps xmm0, xmmword ptr [eax]
			mov ecx, dword ptr Max
			movaps xmm1, xmmword ptr [ecx]

			mov edx, dword ptr m_Points
            add edx, i
			movaps xmm2, xmmword ptr [edx]

			minps xmm0, xmm2
			maxps xmm1, xmm2

			movaps xmmword ptr [eax], xmm0
			movaps xmmword ptr [ecx], xmm1
		}
	}
}

//mVoid CPointArray::CalcAABBox( CAABox &a_Res )
//{
//    mUInt i;
//    CVector4f &vMin = a_Res.Min(), &vMax = a_Res.Max();
//    vMax = vMin = *m_Points.begin();
//    for each (CVector4f vPoint in m_Points)
//    {
//        for (i = 0; i < 3; ++i)
//        {
//            if (vMax[i] < vPoint[i])
//                vMax[i] = vPoint[i];
//
//            if (vMin[i] > vPoint[i])
//                vMin[i] = vPoint[i];
//        }
//    }    
//}

CPointArray::~CPointArray()
{
	//m_Points.clear();
}

mInt32 alikeVector4(const CVector4f &a_V1, const CVector4f &a_V2, const mFloat epsilon)
{
	return (
		com::alike(a_V1[0], a_V2[0], epsilon) &&
		com::alike(a_V1[1], a_V2[1], epsilon) &&
		com::alike(a_V1[2], a_V2[2], epsilon) );
}

CVector4f ZShootRay(CVector4f &a_V1, CVector4f &a_V2, CVector4f &a_V3, CVector4f &a_Point)
{
	CVector4f Norm = (a_V2 - a_V1) ^ (a_V3 - a_V1);
	return CVector4f(a_Point[0], a_Point[1], (a_V1 - a_Point) * Norm / Norm[3], 1.0f);
}